
#include <stdio.h>
#include <string.h>
#include "Nano100Series.h"
#include "bootloader.h"
#include "xprintf.h"

void SendChar_ToUART(unsigned char ch)
{
    while(UART0->FSR & UART_FSR_TX_FULL_F_Msk);
    UART0->THR = ch;
}

void SYS_Init(void)
{
    SYS_UnlockReg();

    /* HXT:12 MHz, PLL:96 MHz, HCLK:32 MHz */
    CLK_EnableXtalRC(CLK_PWRCTL_HXT_EN_Msk);
    CLK_EnablePLL(CLK_PLLCTL_PLL_SRC_HXT, 96000000);
    CLK_WaitClockReady(CLK_CLKSTATUS_HXT_STB_Msk | CLK_CLKSTATUS_PLL_STB_Msk);
    CLK_SetHCLK(CLK_CLKSEL0_HCLK_S_PLL, CLK_HCLK_CLK_DIVIDER(3));
	SystemCoreClockUpdate();

	/* UART0 */
	CLK_SetModuleClock(UART0_MODULE, CLK_CLKSEL1_UART_S_HXT, CLK_UART_CLK_DIVIDER(1));
	CLK_EnableModuleClock(UART0_MODULE);
    SYS->PB_L_MFP &= ~(SYS_PB_L_MFP_PB0_MFP_Msk | SYS_PB_L_MFP_PB1_MFP_Msk);
    SYS->PB_L_MFP |= (SYS_PB_L_MFP_PB0_MFP_UART0_RX | SYS_PB_L_MFP_PB1_MFP_UART0_TX);

	/* DMA (CRC) */
	CLK_EnableModuleClock(DMA_MODULE);
	
    SYS_LockReg();
}

void InitChipConfig(void)
{
	int i;
	uint32_t chip_config[2];
	uint32_t chip_config_default[2] = {
		/* CWDT       CFOSC         CBORST       CBS          LOCK        DFEN */
		(1UL << 31) | (1UL << 26) | (3UL << 19) | (0UL << 6) | (1UL << 1) | (0UL << 0),
		DF_BASE
	};
	
	SYS_UnlockReg();
	FMC_Open();
	
	debug("chip config default\n");
	for (i = 0; i < 2; i++) {
		debug("chip_config_default[%d] : 0x%08X\n", i, chip_config_default[i]);
	}
	
	debug("read chip config current 1 times\n");
	FMC_ReadConfig(chip_config, 2);
	for (i = 0; i < 2; i++) {
		info("chip_config[%d] : 0x%08X\n", i, chip_config[i]);
	}

	for (i = 0; i < 2; i++) {
		if (chip_config[i] != chip_config_default[i]) {
			debug("set chip config to chip_config_default...\n");
			FMC_WriteConfig(chip_config_default, 2);
			debug("read chip config current 2 times\n");
			FMC_ReadConfig(chip_config, 2);
			for (i = 0; i < 2; i++) {
				debug("chip_config[%d] : 0x%08X\n", i, chip_config[i]);
			}
			info("reset\n");
			SYS_ResetChip();
			break;
		}
	}

	FMC_Close();
	SYS_LockReg();
}

uint32_t CRC32(uint8_t *addr, uint32_t count)
{
	int i;
	uint32_t crc;
	
	CRC_Open(CRC_32, CRC_WDATA_RVS | CRC_CHECKSUM_RVS, 0xFFFFFFFF, CRC_CPU_WDATA_8);

    for (i = 0; i < count; i++) {
        CRC_WRITE_DATA(addr[i]);
    }

	crc = CRC_GetChecksum();
	crc ^= 0xFFFFFFFF;
	
    return crc;
}

void ReadShare(T_Share *share)
{
	memcpy(share, (uint32_t *)SHARE_ENTRY, sizeof(T_Share));
}

void WriteShare(T_Share *share)
{
	int i;
	uint32_t *pdata;
	
	SYS_UnlockReg();
	FMC_Open();
	FMC_Erase(SHARE_ENTRY);
	pdata = (uint32_t *)share;
	for (i = 0; i < sizeof(T_Share) / 4; i++) {
		FMC_Write(SHARE_ENTRY + i * 4, pdata[i]);
	}
	FMC_Close();
	SYS_LockReg();
}

__asm __INLINE void SetSp(uint32_t sp)
{
	MSR MSP, r0
	BX lr
}

__INLINE void BranchTo(uint32_t u32Address)
{
    FUNC_ENTRY *func;

	SYS_UnlockReg();
	FMC_Open();
    FMC_SetVectorPageAddr(u32Address);
	FMC_Close();
	SYS_LockReg();
	
    func = (FUNC_ENTRY *)(*(uint32_t *)(u32Address + 4));
	info("branch to 0x%08X\n", (uint32_t)func);
    SetSp(*(uint32_t *)u32Address);
    func();
}

int32_t main (void)
{
	T_Share share;
	uint32_t crc;
	int i;
	int n_page;
	int n_word;
	int n_err;

	// init system
    SYS_Init();
    UART_Open(UART0, 115200);
	xdev_out(SendChar_ToUART);
	info("\n-Bootloader Enter-\n");
	
	// ensure chip config
	InitChipConfig();

	// check share
	ReadShare(&share);
	crc = CRC32((uint8_t *)(&share), sizeof(T_Share) - 4);
	if (crc != share.this_crc) {
		info("share crc error\n");
		goto exit_start_ap;
	}

	// check need_update
	if (!share.need_update) {
		info("do not need update\n");
		goto exit_start_ap;
	}
	
	info("need update\n");

	// check fw_size
	if (share.fw_size > AP_SIZE) {
		info("fw size error : %u\n", share.fw_size);
		goto exit_start_ap;
	}

	// check fw_crc
	crc = CRC32((uint8_t *)FW_ENTRY, share.fw_size);
	if (crc != share.fw_crc) {
		info("fw crc error\n");
		goto exit_start_ap;
	}

	// update ap with fw
	n_err = 0;
try_update:	
	info("try update %d times, fw size : %u\n", n_err, share.fw_size);
	
	SYS_UnlockReg();
	FMC_Open();
	
	FMC_ENABLE_AP_UPDATE();
	n_page = (share.fw_size + FMC_FLASH_PAGE_SIZE - 1) / FMC_FLASH_PAGE_SIZE;
	for (i = 0; i < n_page; i++) {
		FMC_Erase(AP_ENTRY + i * FMC_FLASH_PAGE_SIZE);
	}

	n_word = (share.fw_size + 4 - 1) / 4;
	for (i = 0; i < n_word; i++) {
		FMC_Write((uint32_t)(((uint32_t *)AP_ENTRY) + i), *(((uint32_t *)FW_ENTRY) + i));
	}
	FMC_DISABLE_AP_UPDATE();

	// get ap crc
	FMC_SetVectorPageAddr(AP_ENTRY);
	crc = CRC32((uint8_t *)AP_ENTRY, share.fw_size);
	FMC_SetVectorPageAddr(BL_ENTRY);
	
	FMC_Close();
	SYS_LockReg();

	// check ap
	if (crc != share.fw_crc) {
		info("ap crc error\n");
		n_err++;
		if (n_err <= 3) {
			goto try_update;
		}
		info("flash failure, hang up!\n");
		goto exit_hang_up;
	}
	
exit_start_ap:
	BranchTo(AP_ENTRY);
	
exit_hang_up:
    while(1);
}

